/*
   StepperDriver.cpp - Library for controlling a stepper motor driver that takes
   a square wave pulse signal on a STEP pin to step. This library was developed
   specifically for the Arduino Mega 2560 r3 to interface with a Pololu DRV8825
   Stepper Motor Driver Carrier, High Current (md20a).
   
   Created by the STA Capstone team at UMBC on 17 January 2013.

   The FLT pin on the chip indicates a fault and may be monitored by the
   main loop and attached to an interrupt. This functionality has not been
   implemented as of 14 May 2013.
  
   Enable (EN) pin:
      On the DRV8825, the EN pin is low when the driver is enabled. See
      function descriptions to determine if that function enables and disables
      the driver.
      
   Step mode pins (M0, M1, and M2)
      The values on each of the M0, M1, and M2 pins determine the microstep
      resolution of the motor driver. Values on the M0, M1, and M2 should be as
      follows:
         MODE0 MODE1 MODE2 Microstep Resolution
         Low   Low   Low   Full step
         High  Low   Low   Half step
         Low   High  Low   1/4 step
         High  High  Low   1/8 step
         Low   Low   High  1/16 step
         High  Low   High  1/32 step
         Low   High  High  1/32 step
         High  High  High  1/32 step
   
   SPS and SPSPS
      As noted in each use, SPS and SPSPS may refer to full or micro steps 
      per second (speed) and steps per second per step (acceleration). SPSPS 
      indicates that with each successive step speed will increase by SPSPS.
*/

#include "Arduino.h"
#include "StepperDriver.h"

#include <cmath>

#define VERSION 1 // version of this library

#define ABSOLUTEMAXSPS 1000          // maximum full steps per second due to millisecond time resolution
#define MAXSPS_DEFAULT 1000          // max steps per second
#define MINSPS_DEFAULT 10            // min steps per second
#define MAXSPSPS_DEFAULT 25          // max steps per second per step
#define SMALLESTSTEPMODE 5
#define STEPMODEDEFAULT_DEFAULT 1    // default step mode
#define STEPSPERROTATION_DEFAULT 200 // default steps per rotation of the motor
#define DEGPERSTEP_DEFAULT 1.8       // default degrees per step

#define STEPPULSEDURATION 4   // in microseconds
#define ENABLEDELAY 50        // in milliseconds. Delay after enabling and delay before disabling.

#define MICROSINSECONDS 1000000
#define MICROSINMILLIS 1000
#define MILLISINSECONDS 1000

#define getStepsInStage(x) (stages[x][2]/abs(stages[x][3])) // used for smartStep

#define DEFAULTSPSPS 1  //DEFAULTSPSPS / DEFAULTSPS should be <= 300
#define DEFAULTSPS 100
#define PRECISIONSPS DEFAULTSPS


static const int stepModeMultiplierToMicro[8] = {32,16,8,4,2,1,1,1};
static const int stepModeMultiplierToFull[8] = {1,2,4,8,16,32,32,32};
static long stages[StepperDriver::numStages][4] = 
            {{0,5,DEFAULTSPS,DEFAULTSPSPS},
             {0,4,DEFAULTSPS,DEFAULTSPSPS},
             {0,3,DEFAULTSPS,DEFAULTSPSPS},
             {0,2,DEFAULTSPS,DEFAULTSPSPS},
             {0,1,DEFAULTSPS,DEFAULTSPSPS},
             {0,0,DEFAULTSPS,0},
             {0,1,DEFAULTSPS,-DEFAULTSPSPS},
             {0,2,DEFAULTSPS,-DEFAULTSPSPS},
             {0,3,DEFAULTSPS,-DEFAULTSPSPS},
             {0,4,DEFAULTSPS,-DEFAULTSPSPS},
             {0,5,DEFAULTSPS,-DEFAULTSPSPS},
             {0,5,PRECISIONSPS,0}};

/******************************************************************************
   Constructors
******************************************************************************/

/*
   StepperDriver constructor
   
      Required input parameters:
      - Microcontroller pin numbers (int) that connect to STEP, DIR, SLP, RST,
      M0, M1, M2, and EN on the driver (in that order).
 */
StepperDriver::StepperDriver(int oPin_STEP, int oPin_DIR, int oPin_SLP, 
   int oPin_RST, int oPin_M0, int oPin_M1, int oPin_M2, int oPin_EN)
{
   this->direction = CLOCKWISE;       // motor direction
   this->stepMode = STEPMODEDEFAULT_DEFAULT;
   this->timeLastStep = 0;     // time stamp in us of the last step taken

   this->motorMaxSPS = MAXSPS_DEFAULT;
   this->motorMinSPS = MINSPS_DEFAULT;
   this->motorMaxSPSPS = MAXSPSPS_DEFAULT;

   // Arduino pins for the motor control connection:
   this->oPin_STEP = oPin_STEP;
   this->oPin_DIR  = oPin_DIR;
   this->oPin_SLP = oPin_SLP;
   this->oPin_RST = oPin_RST;
   this->oPin_M0   = oPin_M0;
   this->oPin_M1   = oPin_M1;
   this->oPin_M2   = oPin_M2;
   this->oPin_EN   = oPin_EN;

   // setup the pins on the microcontroller:
   pinMode(this->oPin_STEP, OUTPUT);
   pinMode(this->oPin_DIR, OUTPUT);
   pinMode(this->oPin_SLP, OUTPUT);
   pinMode(this->oPin_RST, OUTPUT);
   pinMode(this->oPin_M0, OUTPUT);
   pinMode(this->oPin_M1, OUTPUT);
   pinMode(this->oPin_M2, OUTPUT);
   pinMode(this->oPin_EN, OUTPUT);
   
   initializeDriver();
}

/******************************************************************************
   Stepping functions
******************************************************************************/
/*
   smartStep
   Executes numSteps steps with acceleration, steady, deceleration, 
   and precision stages of stepping. Number of steps completed will be within
   1/32 = .03125 of a step of numSteps. Remainder steps not completed will be 
   tracked and added to next step operation.
   
   Required input parameter:
   - Number of 1/32 micro steps to move
 */
long StepperDriver::smartStep(long numSteps)
{
   return smartStep(numSteps, DEFAULTSPS, DEFAULTSPSPS);
}

/*
   smartStep
   Executes numSteps steps with acceleration, steady, deceleration, 
   and precision stages of stepping. Number of steps completed will be within
   1/32 = .03125 of a step of numSteps. Remainder steps not completed will be 
   tracked and added to next step operation.
   Input and output of this function will refer to 1/32 steps.
   
   Required input parameter:
   - Number of 1/32 micro steps to move.
   Optional input parameters:
   - Speed in steps per second (SPS) at which to start each stage.
   - Rate of acceleration in steps per second per step (SPSPS) in each stage.
   
   Returns -1 on error.
   Returns number of 1/32 micro-steps completed.
 */
long StepperDriver::smartStep(long numSteps, int sPS, int sPSPS)
{
   if (sPS < 0 || sPS > this->motorMaxSPS) return -1;
   if (sPSPS < 0 || sPSPS > this->motorMaxSPSPS) return -1;
   
   long stepsLeft = numSteps;   // steps left to move
   long stepsLeftToPlan = numSteps;   // steps left to plan
   
   /* 
      Set stages of acceleration
         stages[][0] = number of steps of step mode stages[][1] in stage
         stages[][1] = step mode in stage
         stages[][2] = starting SPS in stage
         stages[][3] = rate of acceleration (+/-SPSPS) in stage
         
         Last stage will finish any final small steps needed.
    */
   int relStage; // deceleration stage corresponding to the ith acceleration stage
   for (int i = 0; i < this->steadyStageIndex; i++)
   {
      relStage = this->numStages - i - 2;
      // initialize num steps in each stage to 0
      stages[i][2] = sPS;
      stages[i][3] = sPSPS;
      stages[i][0] = getStepsInStage(i); // using the macro
      
      if (stepsLeftToPlan/stepModeMultiplierToMicro[stages[i][1]] <= stages[i][0]*2) {
         stages[i][0] = floor(stepsLeftToPlan*.5/stepModeMultiplierToMicro[stages[i][1]]);
      }
      
      stages[relStage][2] = stages[i][2] + stages[i][3]*stages[i][0]; // start at ending speed of relative acceleration stage
      stages[relStage][3] = -sPSPS; // symmetrical but opposite sign
      stages[relStage][0] = stages[i][0]; // perform same number of steps as done in acceleration stage
      
      //Serial.println(stepsLeftToPlan);
      stepsLeftToPlan -= 2 * stages[i][0] * stepModeMultiplierToMicro[stages[i][1]];
      //stepsLeftToPlan -= stages[relStage][0] * stepModeMultiplierToMicro[stages[relStage][1]]; // not necessary if previous is done twice
   }

   // put remaining full steps in the steady stage
   stages[this->steadyStageIndex][2] = sPS; // 
   stages[this->steadyStageIndex][3] = 0;
   stages[this->steadyStageIndex][0] = floor(stepsLeftToPlan / stepModeMultiplierToMicro[stages[this->steadyStageIndex][1]]);
   stepsLeftToPlan -= stages[this->steadyStageIndex][0] * stepModeMultiplierToMicro[stages[steadyStageIndex][1]];
   
   // put remaining micro steps in the precision stage
   stages[this->precisionStageIndex][2] = sPS;
   stages[this->precisionStageIndex][3] = 0;
   stages[this->precisionStageIndex][0] = stepsLeftToPlan;
   stepsLeftToPlan -= stages[this->precisionStageIndex][0];
   
   
   //print out the stages
   Serial.println("Stage values: # steps, step mode, SPS, SPSPS");
   for (int i = 0; i < this->numStages; i++) {
      Serial.print("Stage ");
      Serial.print(i);
      Serial.print(": ");
      Serial.print(stages[i][0]);
      Serial.print(" ");
      Serial.print(stages[i][1]);
      Serial.print(" ");
      Serial.print(stages[i][2]);
      Serial.print(" ");
      Serial.print(stages[i][3]);
      Serial.println();
   }
   
   // Move according to each stage
   enableDriver();
   delay(ENABLEDELAY);
   for (int i = 0; i < numStages; i++) {
      if (stages[i][0] > 0)
         stepsLeft -= stepAccelerate(stages[i][0], stages[i][1], stages[i][2], stages[i][3]) * stepModeMultiplierToMicro[stages[i][1]];
   }
   delay(ENABLEDELAY);
   disableDriver();
   
   return numSteps - stepsLeft;
}


/*
   stepAccelerate
   Accelerates for numSteps steps at sPSPS rate of acceleration starting at 
   startingSPS speed.
   
   Note: Driver must be enabled before calling this function. Disable driver 
   after this function executes.
   
   Required input parameters:
   - Number of steps of size indicated by step mode.
   - Step mode (see header comments for definition of step mode).
   - Starting speed in full steps per second (SPS).
   - Stepping acceleration in full steps per second per step (SPSPS).
*/
long StepperDriver::stepAccelerate(long numSteps, int stepMode, int startingSPS, int sPSPS)
{
   if (stepMode > 7 || stepMode < 0) return -1;
   if (startingSPS < 0) return -1;
   if (startingSPS > this->motorMaxSPS)
      startingSPS = this->motorMaxSPS;
   else if (startingSPS < this->motorMinSPS)
      startingSPS = this->motorMinSPS;
   if (abs(sPSPS) > this->motorMaxSPSPS)
      sPSPS = this->motorMaxSPSPS * (sPSPS/abs(sPSPS));
   
   long stepsLeft = numSteps;  // how many steps to take
   int currSPS = startingSPS;
   int stepDelay;
   setStepMode(stepMode);
   
   while(stepsLeft > 0) {
      stepDelay = MICROSINSECONDS / currSPS;
      
      // disable the driver if delay is too long
      if (stepDelay > 2*ENABLEDELAY) {
         delay(ENABLEDELAY);
         disableDriver();
         delay(stepDelay - 2*ENABLEDELAY);
         enableDriver();
         delay(ENABLEDELAY);
      }
   
      // move only if the appropriate delay has passed
      if (micros() - this->timeLastStep >= stepDelay) {
         // decrement the steps left
         stepsLeft--;
         // step the motor
         stepMotor();
         
         // increase currSPS or set to min or max SPS
         if (currSPS + sPSPS > this->motorMaxSPS)
            currSPS = this->motorMaxSPS;
         else if (currSPS + sPSPS < this->motorMinSPS)
            currSPS = this->motorMinSPS;
         else
            currSPS += sPSPS;
      }
  }
  
  return numSteps - stepsLeft;
}

/*
   step
   Moves the motor numSteps steps at a constant speed given by sPS.
   
   Note: Driver must be enabled before calling this function. Disable driver 
   after this function executes.
   
   Required input parameters:
   - Number of steps of size indicated by step mode.
   - Step mode (see header comments for definition of step mode).
   - Stepping speed in steps per second (SPS).
 */
long StepperDriver::step(long numSteps, int stepMode, int sPS)
{

   if (stepMode > 7 || stepMode < 0) return -1;
   if (sPS < 0) return -1;
   if (sPS > this->motorMaxSPS)
      sPS = this->motorMaxSPS;
   else if (sPS < this->motorMinSPS)
      sPS = this->motorMinSPS;

   long stepsLeft = numSteps;  // how many steps to take
   long stepDelay = MICROSINSECONDS / sPS;
   setStepMode(stepMode);
   enableDriver();
   delay(100);
   while(stepsLeft > 0) {
      // disable the driver if delay is too long
      /*
      if (stepDelay > 2*ENABLEDELAY) {
         delay(ENABLEDELAY);
         disableDriver();
         delay(stepDelay - 2*ENABLEDELAY);
         enableDriver();
         delay(ENABLEDELAY);
      }
      */
      
      // move only if the appropriate delay has passed
      if (micros() - this->timeLastStep >= stepDelay) {
         // decrement the steps left
         stepsLeft--;
         // step the motor, sets this->timeLastStep
         stepMotor();
      }
   }
   delay(100);
   disableDriver();
   
   return numSteps - stepsLeft;
}

/*
   stepMotor
   Executes one step of the motor at current set step mode with current set 
   direction.
*/
int StepperDriver::stepMotor()
{
   this->timeLastStep = micros();
   //send one step signal to STEP pin in current direction
   digitalWrite(this->oPin_STEP,HIGH);
   delayMicroseconds(STEPPULSEDURATION);
   digitalWrite(this->oPin_STEP,LOW);
   
   return 1;
}

/******************************************************************************
   Driver status and settings functions
******************************************************************************/
/*
   setDirection
   Sets the direction of motor rotation
 */
void StepperDriver::setDirection(int direction)
{
   this->direction = direction;
   if (direction == CLOCKWISE) {
      digitalWrite(this->oPin_DIR, HIGH);
      // Serial.println("Direction set to CLOCKWISE");
   } else {
      digitalWrite(this->oPin_DIR, LOW);
      // Serial.println("Direction set to COUNTERCLOCKWISE");
   }
}

/*
   getDirection
   Returns the current direction of the motor.
 */
int StepperDriver::getDirection()
{
   return this->direction;
}

/*
   getTimeLastStep
   Returns the time (long) of the last step.
 */
long StepperDriver::getTimeLastStep()
{
   return this->timeLastStep;
}

/*
   setStepMode
   Sets the step mode. Applies corresponding voltages to driver mode pins.
   Required parameters:
   - Step mode should have integer value of 0 through 7.
   
   Note: Smallest micro-step size is 1/32 step which corresponds with step 
   mode 5. Mode values greater than 5 will still use 1/32 steps.
 */
void StepperDriver::setStepMode(int stepMode)
{
   this->stepMode = stepMode;
   switch (stepMode) 
   {
      case 0:  /* 000 */
         digitalWrite(this->oPin_M0, LOW);
         digitalWrite(this->oPin_M1, LOW);
         digitalWrite(this->oPin_M2, LOW);
         break;
      case 1: /* 001 */
         digitalWrite(this->oPin_M0, HIGH);
         digitalWrite(this->oPin_M1, LOW);
         digitalWrite(this->oPin_M2, LOW);
         break;
      case 2: /* 010 */
         digitalWrite(this->oPin_M0, LOW);
         digitalWrite(this->oPin_M1, HIGH);
         digitalWrite(this->oPin_M2, LOW);
         break;
      case 3: /* 011 */
         digitalWrite(this->oPin_M0, HIGH);
         digitalWrite(this->oPin_M1, HIGH);
         digitalWrite(this->oPin_M2, LOW);
         break;
      case 4: /* 100 */
         digitalWrite(this->oPin_M0, LOW);
         digitalWrite(this->oPin_M1, LOW);
         digitalWrite(this->oPin_M2, HIGH);
         break;
      case 5: /* 101 */
         digitalWrite(this->oPin_M0, HIGH);
         digitalWrite(this->oPin_M1, LOW);
         digitalWrite(this->oPin_M2, HIGH);
         break;
      case 6: /* 110 */
         digitalWrite(this->oPin_M0, LOW);
         digitalWrite(this->oPin_M1, HIGH);
         digitalWrite(this->oPin_M2, HIGH);
         break;
      case 7: /* 111 */
         digitalWrite(this->oPin_M0, HIGH);
         digitalWrite(this->oPin_M1, HIGH);
         digitalWrite(this->oPin_M2, HIGH);
         break;
   }
}

/*
   enableDriver
   Enables the stepper driver.
 */
void StepperDriver::enableDriver()
{
   //digitalWrite(this->oPin_SLP, HIGH);
   digitalWrite(this->oPin_EN, LOW);
   
}

/*
   disableDriver
   Disables the stepper driver.
 */
void StepperDriver::disableDriver()
{
   //digitalWrite(this->oPin_SLP, LOW);
   digitalWrite(this->oPin_EN, HIGH);
}

/*
   initializeDriver
   Initializes the stepper driver and pins.
 */
void StepperDriver::initializeDriver()
{
   setStepMode(this->stepMode);
   digitalWrite(this->oPin_STEP, LOW);
   digitalWrite(this->oPin_SLP, HIGH);
   digitalWrite(this->oPin_RST, HIGH);
   disableDriver();
}

